#include "controller/MotorDriver.hpp"
#include <iostream>
#include <iomanip>
#include <thread>

/**
 * 构造函数：初始化串口
 * @param uart_dev 串口设备路径
 */
MotorDriver::MotorDriver(const std::string &uart_dev) {
    // 初始化串口
    serialComm = std::make_unique<SerialCommunicator>(uart_dev);
}

 /**
  * @brief    读取驱动板参数
  * @param    addr  ：电机地址
  * @param    param     ：参数名称，对应功能码映射表中的键
  * @retval   地址 + 功能码 + 命令状态 + 校验字节
  */
void MotorDriver::readSysParams(uint8_t addr, const std::string &param) {
    std::vector<uint8_t> cmd;
    cmd.push_back(addr);

    // 查找功能码
    auto it = func_code_map.find(param);
    if (it != func_code_map.end()) {
        cmd.push_back(it->second);  // 添加功能码
        if (param == "S_Conf") { // 读取驱动参数，功能码后面还需要加上一个辅助码0x6C
            cmd.push_back(0x6C);
        } else if (param == "S_State") { // 读取系统状态参数，功能码后面还需要加上一个辅助码0x7A
            cmd.push_back(0x7A);
        }
    }

    cmd.push_back(0x6B);  // 校验字节

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * @brief    将当前位置清零
 * @param    addr  ：电机地址
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void MotorDriver::resetCurPosToZero(uint8_t addr) {
    std::array<uint8_t, 4> cmd = {
        addr,    // 地址
        0x0A,    // 功能码
        0x6D,    // 辅助码
        0x6B     // 校验字节
    };

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * @brief    解除堵转保护
 * @param    addr  ：电机地址
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void MotorDriver::resetClogPro(uint8_t addr) {
    std::array<uint8_t, 4> cmd = {
        addr,    // 地址
        0x0E,    // 功能码
        0x52,    // 辅助码
        0x6B     // 校验字节
    };

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * @brief    修改开环/闭环控制模式
 * @param    addr     ：电机地址
 * @param    svF      ：是否存储标志，false为不存储，true为存储
 * @param    ctrl_mode：控制模式（对应屏幕上的P_Pul菜单），0是关闭脉冲输入引脚，1是开环模式，2是闭环模式，3是让En端口复用为多圈限位开关输入引脚，Dir端口复用为到位输出高电平功能
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void MotorDriver::modifyCtrlMode(uint8_t addr, bool svF, uint8_t ctrl_mode) {
    std::array<uint8_t, 6> cmd = {
        addr,                   // 地址
        0x46,                   // 功能码
        0x69,                   // 辅助码
        svF ? 0x01 : 0x00,      // 是否存储标志，false为不存储，true为存储
        ctrl_mode,              // 控制模式（对应屏幕上的P_Pul菜单），0是关闭脉冲输入引脚，1是开环模式，2是闭环模式，3是让En端口复用为多圈限位开关输入引脚，Dir端口复用为到位输出高电平功能
        0x6B                    // 校验字节
    };

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * @brief    使能信号控制
 * @param    addr  ：电机地址
 * @param    state ：使能状态     ，true为使能电机，false为关闭电机
 * @param    snF   ：多机同步标志 ，false为不启用，true为启用
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void MotorDriver::enControl(uint8_t addr, bool state, bool snF) {
    std::array<uint8_t, 6> cmd = { 0 };
    cmd[0] = addr;                    // 地址
    cmd[1] = 0xF3;                    // 功能码
    cmd[2] = 0xAB;                    // 辅助码
    cmd[3] = state ? 0x01 : 0x00;     // 使能状态
    cmd[4] = snF ? 0x01 : 0x00;       // 多机同步运动标志
    cmd[5] = 0x6B;                    // 校验字节

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * @brief    速度模式
 * @param    addr：电机地址
 * @param    dir ：方向       ，0为CW，其余值为CCW
 * @param    vel ：速度       ，范围0 - 5000RPM
 * @param    acc ：加速度     ，范围0 - 255，注意：0是直接启动
 * @param    snF ：多机同步标志，false为不启用，true为启用
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void MotorDriver::velControl(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, bool snF) {
    std::array<uint8_t, 8> cmd = { 0 };
    cmd[0] = addr;                  // 地址
    cmd[1] = 0xF6;                  // 功能码
    cmd[2] = dir;                   // 方向
    cmd[3] = (vel >> 8) & 0xFF;     // 速度(RPM)高8位字节
    cmd[4] = vel & 0xFF;            // 速度(RPM)低8位字节
    cmd[5] = acc;                   // 加速度，注意：0是直接启动
    cmd[6] = snF ? 0x01 : 0x00;     // 多机同步运动标志
    cmd[7] = 0x6B;                  // 校验字节

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * @brief    位置模式
 * @param    addr：电机地址
 * @param    dir ：方向        ，0为CW，其余值为CCW
 * @param    vel ：速度(RPM)   ，范围0 - 5000RPM
 * @param    acc ：加速度      ，范围0 - 255，注意：0是直接启动
 * @param    clk ：脉冲数      ，范围0- (2^32 - 1)个
 * @param    raF ：相位/绝对标志，false为相对运动，true为绝对值运动
 * @param    snF ：多机同步标志 ，false为不启用，true为启用
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void MotorDriver::posControl(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF, bool snF) {
    std::array<uint8_t, 13> cmd = { 0 };
    cmd[0] = addr;                      // 地址
    cmd[1] = 0xFD;                      // 功能码
    cmd[2] = dir;                       // 方向
    cmd[3] = (vel >> 8) & 0xFF;         // 速度(RPM)高8位字节
    cmd[4] = vel & 0xFF;                // 速度(RPM)低8位字节
    cmd[5] = acc;                       // 加速度，注意：0是直接启动
    cmd[6] = (clk >> 24) & 0xFF;        // 脉冲数(bit24 - bit31)
    cmd[7] = (clk >> 16) & 0xFF;        // 脉冲数(bit16 - bit23)
    cmd[8] = (clk >> 8) & 0xFF;         // 脉冲数(bit8  - bit15)
    cmd[9] = clk & 0xFF;                // 脉冲数(bit0  - bit7 )
    cmd[10] = raF ? 0x01 : 0x00;        // 相位/绝对标志，false为相对运动，true为绝对值运动
    cmd[11] = snF ? 0x01 : 0x00;        // 多机同步运动标志，false为不启用，true为启用
    cmd[12] = 0x6B;                     // 校验字节

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * @brief    立即停止（所有控制模式都通用）
 * @param    addr  ：电机地址
 * @param    snF   ：多机同步标志，false为不启用，true为启用
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void MotorDriver::stopNow(uint8_t addr, bool snF) {
    std::array<uint8_t, 5> cmd = {
        addr,               // 地址
        0xFE,               // 功能码
        0x98,               // 辅助码
        snF ? 0x01 : 0x00,  // 多机同步运动标志
        0x6B                // 校验字节
    };

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * @brief    多机同步运动
 * @param    addr  ：电机地址
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void MotorDriver::synchronousMotion(uint8_t addr) {
    std::array<uint8_t, 4> cmd = {
        addr,               // 地址
        0xFF,               // 功能码
        0x66,               // 辅助码
        0x6B                // 校验字节
    };

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * @brief    设置单圈回零的零点位置
 * @param    addr  ：电机地址
 * @param    svF   ：是否存储标志，false为不存储，true为存储
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void MotorDriver::originSetO(uint8_t addr, bool svF) {
    std::array<uint8_t, 5> cmd = {
        addr,               // 地址
        0x93,               // 功能码
        0x88,               // 辅助码
        svF ? 0x01 : 0x00,  // 是否存储标志，false为不存储，true为存储
        0x6B                // 校验字节
    };

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * @brief    修改回零参数
 * @param    addr  ：电机地址
 * @param    svF   ：是否存储标志，false为不存储，true为存储
 * @param    o_mode ：回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零
 * @param    o_dir  ：回零方向，0为CW，其余值为CCW
 * @param    o_vel  ：回零速度，单位：RPM（转/分钟）
 * @param    o_tm   ：回零超时时间，单位：毫秒
 * @param    sl_vel ：无限位碰撞回零检测转速，单位：RPM（转/分钟）
 * @param    sl_ma  ：无限位碰撞回零检测电流，单位：Ma（毫安）
 * @param    sl_ms  ：无限位碰撞回零检测时间，单位：Ms（毫秒）
 * @param    potF   ：上电自动触发回零，false为不使能，true为使能
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void MotorDriver::originModifyParams(uint8_t addr, bool svF, uint8_t o_mode, uint8_t o_dir, uint16_t o_vel, uint32_t o_tm,
    uint16_t sl_vel, uint16_t sl_ma, uint16_t sl_ms, bool potF) {
    std::array<uint8_t, 20> cmd = { 0 };
    cmd[0] = addr;                             // 地址
    cmd[1] = 0x4C;                             // 功能码
    cmd[2] = 0xAE;                             // 辅助码
    cmd[3] = svF ? 0x01 : 0x00;                // 是否存储标志，false为不存储，true为存储
    cmd[4] = o_mode;                           // 回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零
    cmd[5] = o_dir;                            // 回零方向
    cmd[6] = (o_vel >> 8) & 0xFF;              // 回零速度(RPM)高8位字节
    cmd[7] = o_vel & 0xFF;                     // 回零速度(RPM)低8位字节
    cmd[8] = (o_tm >> 24) & 0xFF;              // 回零超时时间(bit24 - bit31)
    cmd[9] = (o_tm >> 16) & 0xFF;              // 回零超时时间(bit16 - bit23)
    cmd[10] = (o_tm >> 8) & 0xFF;              // 回零超时时间(bit8  - bit15)
    cmd[11] = o_tm & 0xFF;                     // 回零超时时间(bit0  - bit7 )
    cmd[12] = (sl_vel >> 8) & 0xFF;            // 无限位碰撞回零检测转速(RPM)高8位字节
    cmd[13] = sl_vel & 0xFF;                   // 无限位碰撞回零检测转速(RPM)低8位字节 
    cmd[14] = (sl_ma >> 8) & 0xFF;             // 无限位碰撞回零检测电流(Ma)高8位字节
    cmd[15] = sl_ma & 0xFF;                    // 无限位碰撞回零检测电流(Ma)低8位字节 
    cmd[16] = (sl_ms >> 8) & 0xFF;             // 无限位碰撞回零检测时间(Ms)高8位字节
    cmd[17] = sl_ms & 0xFF;                    // 无限位碰撞回零检测时间(Ms)低8位字节
    cmd[18] = potF ? 0x01 : 0x00;              // 上电自动触发回零，false为不使能，true为使能
    cmd[19] = 0x6B;                            // 校验字节

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * @brief    触发回零
 * @param    addr   ：电机地址
 * @param    o_mode ：回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零
 * @param    snF   ：多机同步标志，false为不启用，true为启用
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void MotorDriver::originTriggerReturn(uint8_t addr, uint8_t o_mode, bool snF) {
    std::array<uint8_t, 5> cmd = {
        addr,               // 地址
        0x9A,               // 功能码
        o_mode,             // 回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零
        snF ? 0x01 : 0x00,  // 多机同步运动标志，false为不启用，true为启用
        0x6B                // 校验字节
    };

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * @brief    强制中断并退出回零
 * @param    addr  ：电机地址
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void MotorDriver::originInterrupt(uint8_t addr) {
    std::array<uint8_t, 4> cmd = {
        addr,    // 地址
        0x9C,    // 功能码
        0x48,    // 辅助码
        0x6B     // 校验字节
    };

    // 发送命令个串口
    std::lock_guard<std::mutex> lock(serialMutex);
    serialComm->sendData(cmd);
}

/**
 * 实时位置监控
 * @return 返回电机的实时位置角度
 */
float MotorDriver::realTimeLocation(uint8_t addr) {

    // 读取电机实时位置
    readSysParams(addr, "S_CPOS");
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    // 接收数据
    auto buffer = serialComm->receiveData();

    // 验证数据有效性并解析位置信息
    if (buffer.size() >= 8 &&
        buffer[0] == addr &&
        buffer[1] == 0x36) {  // 假设实时位置命令返回0x36

        // 解析符号位
        bool isNegative = (buffer[2] == 0x01);

        // 拼接成uint32_t类型的电机位置值
        uint32_t position = (static_cast<uint32_t>(buffer[3]) << 24) |
            (static_cast<uint32_t>(buffer[4]) << 16) |
            (static_cast<uint32_t>(buffer[5]) << 8) |
            static_cast<uint32_t>(buffer[6]);

        // 角度转换
        motorCurPos = static_cast<float>(position) * 360.0f / 65536.0f;
        // 检查符号位
        if (isNegative) {
            motorCurPos = -motorCurPos;
        }

    }
    // 调试输出
    std::cout << "Motor" << static_cast<int>(addr) << ": " << std::fixed << std::setprecision(1) << motorCurPos << "°" << std::endl;

    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    return motorCurPos;
}